/**
@author zhongyang
该模块是将飞控发送来的四元素换成姿态角度
订阅的信息是/mavros/imu/data
发布的信息是attitude，单位是弧度。
*/
#include <ros/ros.h>
#include <stdlib.h>
#include <math.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Header.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Vector3.h>
#include <std_msgs/Float64.h>

//方便调用c++原有的标准函数
using namespace std;

geometry_msgs::Quaternion q;//定义全局的四元素
geometry_msgs::Vector3 attitude;
std_msgs::Float64 epsilon; 
std_msgs::Float64 threshold;
std_msgs::Float64 test;
std_msgs::Float64 PI;

void att_callBack(const sensor_msgs::Imu::ConstPtr &msg)
{
    q = msg->orientation;
}

int Sign(const std_msgs::Float64 x)
{
    if(x.data>0.0)
        return 1;
    else
        return -1;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "attitudeNode");
    ros::NodeHandle nh;
    //订阅传感器的四元素数据
    ros::Subscriber att_p_sub = nh.subscribe("/mavros/imu/data", 50, att_callBack);
    //发布计算好的姿态值
    ros::Publisher att_pub = nh.advertise<geometry_msgs::Vector3>("attitude", 50);

    ros::Rate loop_rate(50);
    //这里的封装类型不能在全局变量的位置赋值，没找着原因。
    epsilon.data = 0.0009765625;
    threshold.data = 0.5 - epsilon.data;//俯仰角接近90度时，对应的一个阈值
    PI.data = 3.1415926535898;

    while(ros::ok())
    {
        cout<<"ready to caculate the attitude"<<endl;
        
        //cout<<q.x<<" "<<q.y<<" "<<q.z<<" "<<q.w<<endl;
        test.data = q.x*q.z - q.w*q.y;//当俯仰角为90度时，奇异姿态。

        if(test.data < (-threshold.data) || test.data > threshold.data)
        {
            int sign = Sign(test);//逻辑回归函数
            attitude.z = -2*sign*atan2(q.x,q.w);
            attitude.y = sign * (PI.data / 2);
            attitude.x = 0;//90度时，不存在滚转角度
        }
        else{
            attitude.x = atan2(2*(q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);//滚转角度
            attitude.y = asin(-2 * (q.x*q.z - q.w*q.y));//俯仰角
            attitude.z = atan2(2 * (q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);//值域在pi到-pi间
        }

        cout<<attitude.x<<" "<<attitude.y<<" "<<attitude.z<<" "<<endl;

        att_pub.publish(attitude);

        ros::spinOnce();//一定要设置spin函数，否则不能回调，得不到数据。
        loop_rate.sleep();

    }

    return 0;
}